#include <rclcpp/rclcpp.hpp>
// #include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>  // Include this header for tf2 Quaternion

class TFPublisher : public rclcpp::Node
{
public:
    TFPublisher()
        : Node("tf_publisher_node")
    {
        // Declare a parameter for the target frame name
        this->declare_parameter<std::string>("target_frame", "map");
        this->declare_parameter<std::string>("robot_ns", "robot_0");

        // Get the target frame from parameters
        target_frame_ = this->get_parameter("target_frame").as_string();
        robot_ns_ = this->get_parameter("robot_ns").as_string();


        // Create a subscription to the /odom topic
        odom_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "odom/unfiltered", 10, std::bind(&TFPublisher::odom_callback, this, std::placeholders::_1));

        // Create a static broadcaster to send out transforms
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        // Create a publisher for PoseStamped messages
        pose_publish_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("pose", 10);
    }

private:
    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        geometry_msgs::msg::TransformStamped transformStamped;

        // Read message content and assign it to
        // transformStamped.transform.translation.x/y/z
        transformStamped.header.stamp = this->get_clock()->now();
        transformStamped.header.frame_id = target_frame_;
        transformStamped.child_frame_id = robot_ns_ + "/base_footprint";

        // transformStamped.transform.translation.x = msg->pose.pose.position.x;
        // transformStamped.transform.translation.y = msg->pose.pose.position.y;
        // transformStamped.transform.translation.z = msg->pose.pose.position.z;

        // tf2::Quaternion q(
        //     msg->pose.pose.orientation.x,
        //     msg->pose.pose.orientation.y,
        //     msg->pose.pose.orientation.z,
        //     msg->pose.pose.orientation.w);

        // transformStamped.transform.rotation.x = q.x();
        // transformStamped.transform.rotation.y = q.y();
        // transformStamped.transform.rotation.z = q.z();
        // transformStamped.transform.rotation.w = q.w();

        transformStamped.transform.translation.x = 0.0;
        transformStamped.transform.translation.y = 0.0;
        transformStamped.transform.translation.z = 0.0;

        tf2::Quaternion q(
            0.0,
            0.0,
            0.0,
            1.0);

        transformStamped.transform.rotation.x = q.x();
        transformStamped.transform.rotation.y = q.y();
        transformStamped.transform.rotation.z = q.z();
        transformStamped.transform.rotation.w = q.w();

        // Send the transformation
        tf_broadcaster_->sendTransform(transformStamped);

        geometry_msgs::msg::PoseStamped pose_msg;
        pose_msg.header = transformStamped.header;
        pose_msg.header.frame_id = "/" + robot_ns_ + "/base_footprint";
        pose_msg.pose = msg->pose.pose;

        pose_publish_->publish(pose_msg);
    }

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscription_;
    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publish_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    std::string target_frame_;
    std::string robot_ns_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<TFPublisher>());
    rclcpp::shutdown();
    return 0;
}

